import matplotlib.pyplot as plt
import math
import numpy
MAX=10000
class Node:
    def __init__(self,x,y):
        self.x=x
        self.y=y
    def is_equal(self,node):
        if isinstance(node,self.__class__):
            if node.x==self.x and node.y==self.y:
                return True
        return False
class Edge:
    def __init__(self,node1:Node,node2:Node):
        self.node1=node1
        self.node2=node2
        self.length=math.sqrt(math.pow(node1.x-node2.x,2)+math.pow(node1.y-node2.y,2))
    def is_intersect(self,edge):
        """
                判断该线段是否和目标线段相交
                这里假设自己是AB，目标线段是CD
                :param edge:
                :return:
                """
        if isinstance(edge,self.__class__):
            A=numpy.array([self.node1.x,self.node1.y])
            B=numpy.array([self.node2.x,self.node2.y])
            C=numpy.array([edge.node1.x,edge.node1.y])
            D=numpy.array([edge.node2.x,edge.node2.y])
            return (numpy.dot(numpy.cross((C-A),(D-A)),numpy.cross((C-B),(D-B)))<0 and
                    numpy.dot(numpy.cross((A-C),(B-C)),numpy.cross((A-D),(B-D)))<0
                    )
    def is_equal(self,edge):
        if isinstance(edge,self.__class__):
            if edge.node1.is_equal(self.node1) and edge.node2.is_equal(self.node2):
                return True
            if edge.node1.is_equal(self.node2) and edge.node2.is_equal(self.node1):
                return True
        return False
class Polygon:
    def __init__(self, edges):
        self.edges = edges
        self.nodes=[]
        for edge in self.edges:
            if edge.node1 not in self.nodes:
                self.nodes.append(edge.node1)
            if edge.node2 not in self.nodes:
                self.nodes.append(edge.node2)
class Problem:
    def __init__(self,initial:Node,goal:Node,nodes,edges,polygons):
        """
        :param initial:起始点,
        :param goal: 重点
        :param nodes: 所有点的坐标
        :param edges: 所有障碍物边的坐标
        """
        self.nodes=nodes
        self.nodes.insert(0,initial)#起始点在邻接矩阵第0位
        self.nodes.insert(1,goal)#终点在邻接矩阵第1位
        self.state = 0#用来记录当前位置，初始为起点，也就是0
        self.goal = 1
        self.length=len(self.nodes)
        self.edges=edges
        self.polygons=polygons
        self.martix=self.creat_martix()
        self.martix[0][1]=MAX*100#初始化要比普通的MAX还要大
        self.node_explored=[initial,]
        self.path=[0,]
        self.grade=0
    def creat_martix(self):
        length=len(self.nodes)
        matrix = [[MAX for _ in range(length)] for _ in range(length)]
        for i in range(length):
            matrix[i][i] = 0
        for i in range(length):
            for j in range(i + 1, length):
                x = 0  # 判断两点之间的线段是否和其他线段相交，默认不相交
                y = 0  # 判断nodes[i]是否在该多边形中
                z = 0  # 判断nodes[j]是否在该多边形中
                for polygon in self.polygons:
                    # 这一步判断这两个点是否属于同一个多边形
                    for node in polygon.nodes:
                        if nodes[i].is_equal(node):
                            y = 1
                        if nodes[j].is_equal(node):
                            z = 1
                    if y == 1 and z == 1:
                        # 记录该多边形，之后用
                        polygon_mark = polygon
                        break
                    else:
                        y = 0
                        z = 0
                if y == 1 and z == 1:
                    # 如果是在相同多边形进一步判断是否属于邻点,默认不是邻点
                    for edge in polygon_mark.edges:
                        # print(nodes[i].x, nodes[i].y, "--", nodes[j].x, nodes[j].y)
                        # print(edge.node1.x,edge.node1.y,"--",edge.node2.x,edge.node2.y)
                        # print(Edge(nodes[i], nodes[j]).is_equal(edge))
                        # print(Edge(nodes[j], nodes[i]).is_equal(edge))
                        if Edge(nodes[i], nodes[j]).is_equal(edge) or Edge(nodes[j], nodes[i]).is_equal(edge):
                            # print(nodes[i].x, nodes[i].y, "--", nodes[j].x, nodes[j].y)
                            x = 1
                            matrix[i][j] = Edge(nodes[i], nodes[j]).length
                            matrix[j][i] = Edge(nodes[i], nodes[j]).length
                else:
                    for edge in edges:
                        if edge.is_intersect(Edge(nodes[i], nodes[j])):
                            x = 1
                    if x == 0:
                        matrix[i][j] = Edge(nodes[i], nodes[j]).length
                        matrix[j][i] = Edge(nodes[i], nodes[j]).length
        return matrix

    def run(self):
        """
        :return:返回路径的列表
        """
        while True:
            tem_grade=MAX*100#初始化要比普通的MAX还要大
            if self.martix[self.state][1]<MAX:
                self.path.append(1)
                break
            for i in range(1,self.length):
                if i in self.path:
                    continue
                grade=self.grade+self.martix[self.state][i]+Edge(node1=nodes[i],node2=nodes[1]).length

                if  grade<tem_grade:
                    tem_grade=grade
                    tem_node=i
            if tem_grade<self.grade+self.martix[self.state][1]:
                self.grade=tem_grade
                self.state=tem_node
                self.path.append(tem_node)


nodes=[Node(44,260),Node(211,260),Node(211,195),Node(44,195), #横矩形
       Node(88,10),Node(27,78),Node(37,146),Node(94,163),Node(128,75), #五边形
       Node(127,169),Node(150,64),Node(175,169), #上三角
       Node(178,98),Node(178,16),Node(219,10),Node(248,48), #左四边形
       Node(219,135),Node(235,229),Node(270,187), #下三角
       Node(327,154),Node(257,154),Node(258,17),Node(327,17), #纵矩形
       Node(371,189),Node(336,35),Node(361,15),Node(382,40), #右四边形
       Node(327,265),Node(296,240),Node(296,192),Node(335,161),Node(363,192),Node(363,243),] #六边形
edges=[Edge(Node(44,260),Node(211,260)),Edge(Node(211,260),Node(211,195)),Edge(Node(211,195),Node(44,195)),
       Edge(Node(44,260),Node(44,195)), #横矩形
       Edge(Node(88,10),Node(27,78)),Edge(Node(27,78),Node(37,146)),Edge(Node(37,146),Node(94,163)),
       Edge(Node(94,163),Node(128,75)),Edge(Node(88,10),Node(128,75)), #五边形
       Edge(Node(127,169),Node(150,64)),Edge(Node(150,64),Node(175,169)),Edge(Node(127,169),Node(175,169)), #上三角
       Edge(Node(178,98),Node(178,16)),Edge(Node(178,16),Node(219,10)),Edge(Node(219,10),Node(248,48)),
       Edge(Node(178,98),Node(248,48)), #左四边形
       Edge(Node(219,135),Node(235,229)),Edge(Node(235,229),Node(270,187)),Edge(Node(219,135),Node(270,187)), #下三角
       Edge(Node(327,154),Node(257,154)),Edge(Node(257,154),Node(258,17)),Edge(Node(258,17),Node(327,17)),
       Edge(Node(327,154),Node(327,17)), #纵矩形
       Edge(Node(371,189),Node(336,35)),Edge(Node(336,35),Node(361,15)),Edge(Node(361,15),Node(382,40)),
       Edge(Node(371,189),Node(382,40)), #右四边形
       Edge(Node(327,265),Node(296,240)),Edge(Node(296,240),Node(296,192)),Edge(Node(296,192),Node(335,161)),
       Edge(Node(335,161),Node(363,192)),Edge(Node(363,192),Node(363,243)),Edge(Node(327,265),Node(363,243))] #六边形

polygons=[Polygon([Edge(Node(44,260),Node(211,260)),Edge(Node(211,260),Node(211,195)),Edge(Node(211,195),Node(44,195)),
       Edge(Node(44,260),Node(44,195))]),#横矩形
        Polygon([Edge(Node(88,10),Node(27,78)),Edge(Node(27,78),Node(37,146)),Edge(Node(37,146),Node(94,163)),
       Edge(Node(94,163),Node(128,75)),Edge(Node(88,10),Node(128,75))]),#五边形
          Polygon([Edge(Node(127,169),Node(150,64)),Edge(Node(150,64),Node(175,169)),
                   Edge(Node(127,169),Node(175,169))]),#上三角
          Polygon([Edge(Node(178,98),Node(178,16)),Edge(Node(178,16),Node(219,10)),Edge(Node(219,10),Node(248,48)),
                    Edge(Node(178,98),Node(248,48))]),#左四边形
          Polygon([Edge(Node(219,135),Node(235,229)),Edge(Node(235,229),Node(270,187))
                      ,Edge(Node(219,135),Node(270,187)),]), #下三角
          Polygon([Edge(Node(327,154),Node(257,154)),Edge(Node(257,154),Node(258,17)),Edge(Node(258,17),Node(327,17)),
       Edge(Node(327,154),Node(327,17)), ]),#纵矩形
          Polygon([Edge(Node(371,189),Node(336,35)),Edge(Node(336,35),Node(361,15)),Edge(Node(361,15),Node(382,40)),
       Edge(Node(371,189),Node(382,40)),]),#右四边形
          Polygon([Edge(Node(327,265),Node(296,240)),Edge(Node(296,240),Node(296,192)),Edge(Node(296,192),Node(335,161)),
       Edge(Node(335,161),Node(363,192)),Edge(Node(363,192),Node(363,243)),Edge(Node(327,265),Node(363,243))])#六边形
          ]

initial_Node=Node(30, 233)
goal_Node=Node(436,21)
problem=Problem(initial_Node,goal_Node,nodes,edges,polygons)
problem.run()
fig = plt.figure()
for edge in edges:
    plt.plot([edge.node1.x,edge.node2.x],[edge.node1.y,edge.node2.y],color="blue")
length=len(problem.path)
grade=0
for i in range(length):
    if i==length-1:
        break
    edge=Edge(node1=nodes[problem.path[i]],node2=nodes[problem.path[i+1]])
    grade+=edge.length
    plt.plot([edge.node1.x,edge.node2.x],[edge.node1.y,edge.node2.y],color="black")
plt.show()
print(grade)
print(problem.martix)
